
#include <map>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>

//#include "datastructure.h"
#include "point_cloud.h"


#define M_PI 3.14

//// comparative operator for 2 dPoint
struct LessdPoint
{
    public:

        LessdPoint()
        {
        };

        bool operator()(const dPoint& p1, const dPoint& p2)
        {
            for (int i = 0; i < p1.dimension(); i++)
            {
                if (p1[i] < p2[i])
                {
                    return true;
                }
                else
                    if ((p1[i] > p2[i]))
                    {
                        return false;
                    }
            }

            return false;
        }
};



double voronoi_cell_area(std::vector<std::vector<double> >& m_points_2d);
void est_tangent_space_lapack(const dPoint& pt, const vector<dPoint>& neighbor_pts, double h, unsigned int tdim, vector<dVector>& tspace);
void generate_sym_pcd_qb(PCloud& pcloud, double h, double rho, vector<unsigned int>& II, vector<unsigned int>& JJ, vector<double>& SS, vector<double>& BB, vector<vector<int>>& _vector_neighbor_id, vector<vector<dPoint>>& _vector_neighbor_d);
void cal_sym_pcb_qb(PCloud _pc, std::vector<unsigned int>& _m_II, std::vector<unsigned int>& _m_JJ, std::vector<double>& _m_SS, std::vector<double>& _m_BB, vector<vector<int>>& _vector_neighbor_id, vector<vector<dPoint>>& _vector_neighbor_d);

bool save_IJS(const char* filename, std::vector<unsigned int>& II, std::vector<unsigned int>& JJ, std::vector<double>& SS);
bool writeDoubleArray(std::vector<double>& param, const char* filename);



double voronoi_cell_area(std::vector<std::vector<double> >& m_points_2d)
{
    //cerr<<"voronoi_cell_area begin\n";

    std::vector<double> pt;
    // double val;

    VD vd;
    Point_2* pt2;
    Site_2* t;

    BOOST_FOREACH(pt, m_points_2d)
    {
        t = new Site_2(pt[0], pt[1]);
        vd.insert(*t);
        delete t;
    }

    if (vd.is_valid())
    {
        //        printf("voronoi_cell_area: valid \n");
    }
    else
    {
        cerr << "voronoi_cell_area: not valid \n";
        return 0;
    }

    //    printf("voronoi_cell_area: number_of_vertices: %d\n", vd.number_of_vertices());

    Face_handle* fi;
    pt2 = new Point_2(m_points_2d[0][0], m_points_2d[0][1]);
    Locate_result lr = vd.locate(*pt2);
    fi = boost::get<Face_handle > (&lr);
    delete pt2;

    Ccb_halfedge_circulator ec_start = (*fi)->outer_ccb();
    Ccb_halfedge_circulator ec = ec_start;

    Polygon pl;

    CGAL::Cartesian<double>::Point_2* p2;

    double x, y;
    do
    {
        if (!ec->has_source())
        {
            cerr << "voronoi_cell_area : error : no source.\n";
            //delete fi;
            return 0;
        }

        x = ec->source()->point().x();
        y = ec->source()->point().y();
        //        printf("polygon vertices: %f, %f\n", x, y);

        //cerr<<"just before p2 = new CGAL::Cartesian<double>::Point_2(x, y);";
        p2 = new CGAL::Cartesian<double>::Point_2(x, y);
        pl.push_back(*p2);
        delete p2;
    }
    while (++ec != ec_start);

    //delete fi;

    //cerr<<"voronoi_cell_area end\n";
    return pl.area();
}

// int dspev_(char *jobz, char *uplo, integer *n, doublereal *ap, doublereal *w, doublereal *z__, integer *ldz, doublereal *work, integer *info)
//DSPEV computes all the eigenvalues and, optionally, eigenvectors of a
//	real symmetric matrix A in packed storage.
//
//	Arguments
//	=========
//
//	JOBZ    (input) CHARACTER*1
//	        = 'N':  Compute eigenvalues only;
//          = 'V':  Compute eigenvalues and eigenvectors.
//
//	UPLO    (input) CHARACTER*1
//	        = 'U':  Upper triangle of A is stored;
//          = 'L':  Lower triangle of A is stored.
//
//	N       (input) INTEGER
//	         The order of the matrix A.  N >= 0.
//
//	AP      (input/output) DOUBLE PRECISION array, dimension (N*(N+1)/2)
//	         On entry, the upper or lower triangle of the symmetric matrix
//	         A, packed columnwise in a linear array.  The j-th column of A
//	         is stored in the array AP as follows:
//           if UPLO = 'U', AP(i + (j-1)*j/2) = A(i,j) for 1<=i<=j;
//           if UPLO = 'L', AP(i + (j-1)*(2*n-j)/2) = A(i,j) for j<=i<=n.
//
//	         On exit, AP is overwritten by values generated during the
//	         reduction to tridiagonal form.  If UPLO = 'U', the diagonal
//	         and first superdiagonal of the tridiagonal matrix T overwrite
//	         the corresponding elements of A, and if UPLO = 'L', the
//	         diagonal and first subdiagonal of T overwrite the
//	         corresponding elements of A.
//
//	W       (output) DOUBLE PRECISION array, dimension (N)
//	         If INFO = 0, the eigenvalues in ascending order.
//
//	Z       (output) DOUBLE PRECISION array, dimension (LDZ, N)
//	         If JOBZ = 'V', then if INFO = 0, Z contains the orthonormal
//	         eigenvectors of the matrix A, with the i-th column of Z
//	         holding the eigenvector associated with W(i).
//	         If JOBZ = 'N', then Z is not referenced.
//
//	LDZ     (input) INTEGER
//	         The leading dimension of the array Z.  LDZ >= 1, and if
//	JOBZ = 'V', LDZ >= max(1,N).
//
//	WORK    (workspace) DOUBLE PRECISION array, dimension (3*N)
//
//	INFO    (output) INTEGER
//	= 0:  successful exit.
//	< 0:  if INFO = -i, the i-th argument had an illegal value.
//	> 0:  if INFO = i, the algorithm failed to converge; i
//	off-diagonal elements of an intermediate tridiagonal
//	form did not converge to zero.

extern "C" void dspev_(char& jobz, char& uplo,
                       long int& n, double* ap,
                       double* w, double* z,
                       long int& ldz, double* work, long int& info);


////est_tangent_space_lapack(pt, neighbor_pts, h, tdim, tspace);
////assert(index < size && "Index out of bounds")
void est_tangent_space_lapack(const dPoint& pt, const vector<dPoint>& neighbor_pts, double h, unsigned int tdim, vector<dVector>& tspace)
{
    unsigned int dim = pt.dimension();
    assert(tdim <= dim && "tdim > dim !");

    double* mat = new double[(dim + 1) * dim / 2];
    assert(mat != NULL);
    double* wr = new double[dim];
    assert(wr != NULL);
    double* vr = new double[dim * dim];
    assert(vr != NULL);

    double hh = h * h;
    memset(mat, 0, sizeof(double) * (dim + 1) * dim / 2);

    for (unsigned int i = 0; i < neighbor_pts.size(); i++)
    {
        dVector vec = neighbor_pts[i] - pt;
        double wt = exp(-CGAL::to_double(vec * vec) / hh);

        for (unsigned int j = 0; j < dim; j++)
        {
            for (unsigned int k = j; k < dim; k++)
            {
                mat[(k + 1) * k / 2 + j] += wt * vec[j] * vec[k];
            }
        }
    }

    // find out the eigenvalues and eigenvectors.
    char jobz = 'V', uplo = 'U';
    long int n = dim, ldz = dim, info = 0;
    double* work = new double[3 * dim];
    assert(work != NULL);

    dspev_(jobz, uplo, n, mat, wr, vr, ldz, work, info);

    //cout<<"dspev_ info: "<<info<<endl;

    for (unsigned int i = 1; i <= tdim; i++)
    {
        tspace.push_back(dVector(dim, vr + (dim - i) * dim, vr + (dim - i + 1) * dim));
    }


    //cout<<"wr: ";
    //for(unsigned int j = 0; j < dim; j ++){
    //	cout<<wr[j]<<" ";
    //}
    //cout<<endl;

    delete []mat;
    delete []wr;
    delete []vr;
}


////void generate_sym_pcd_qb( //*pc, avgs * 2, 3, m_II, m_JJ, m_SS, m_BB
void generate_sym_pcd_qb(PCloud& pcloud, double h, double rho, vector<unsigned int>& II, vector<unsigned int>& JJ, vector<double>& SS, vector<double>& BB, vector<vector<int>>& _vector_neighbor_id, vector<vector<dPoint>>& _vector_neighbor_d)
{
    printf("generate_pcdlaplace_symmetric_matrix_sparse\n");

    std::vector<std::vector<double>> projected_v;
    std::vector<double> projected_vv;
    projected_vv.resize(2);
    double vc_area;

    typedef CGAL::Search_traits_d<KCd> Traits; //The class Search_traits_d<Kernel> can be used as a template parameter of the kd tree and the search classes.Kernel is CGAL::Cartesian_d<double> KCd
    typedef CGAL::Fair<Traits> Fair;
    typedef CGAL::Kd_tree<Traits, Fair> Tree;
    typedef CGAL::Fuzzy_sphere<Traits> Fuzzy_sphere;
    typedef KCd::FT FT;

    const unsigned int tdim = 2;


    printf("h: %f, rho: %f\n", h, rho);
    double hh = h * h;
    double nmfactor = M_PI * hh * hh / 4;

    printf("nmfactor: %f\n", nmfactor);

    unsigned int np = pcloud.get_cloud_size();
    vector<dPoint> points;
    map<const dPoint, int, LessdPoint> pt2index;

    for (unsigned int i = 0; i < np; i++)
    {
        points.push_back(pcloud.get_point(i));
        pt2index.insert(make_pair(pcloud.get_point(i), i));
    }

    Fair fair(10);
    Tree tree(points.begin(), points.end(), fair);
    points.clear();



    double eps = 1e-3 * h * rho;
    vector<dPoint> neighbor_pts;
    vector<int> neighbor_indices;
    vector<dVector> tspace;
    Triangulation_2 tr;

    // calculate for each point p
    for (unsigned int i = 0; i < np; i++)
    {

        printf("i: %d\r", i);

        dPoint pt = pcloud.get_point(i);

        //search the points within distance h * rho;
        neighbor_pts.clear();
        ////Ball with radius r = 10*rho
        Fuzzy_sphere fs(pt, h * rho, FT(eps));
        tree.search(back_insert_iterator<vector<dPoint> >(neighbor_pts), fs);

		//// Save all neighbor of point pt for heat method
        _vector_neighbor_d.push_back(neighbor_pts);

        //look up the indices of the neighboring points
        neighbor_indices.clear();

        bool find = false;

        for (vector<dPoint>::iterator iter = neighbor_pts.begin(); iter != neighbor_pts.end(); iter++)
        {
            //cout<<*iter<<endl;
            map<const dPoint, int, LessdPoint>::iterator
            iter_pt2index = pt2index.find(*iter);

            if (iter_pt2index != pt2index.end())
            {
                neighbor_indices.push_back(iter_pt2index->second);

                if (iter_pt2index->second == (int) i)
                {
                    find = true;
                }
            }
            else
            {
                //cout<<"Warning: Fail to find the index of the neighboring points!!"<<endl;
                printf("generate_sym_pcd_qb: Failed to find the index of the neighboring points\n");
            }
        }

        assert(neighbor_indices.size() >= 3 && find);

        if ((neighbor_indices.size() < 3) || !find)
        {
            printf("neighbor_indices.size() < 3 \n");
        }

		//// Save all indecies of neighbor for heat method
        _vector_neighbor_id.push_back(neighbor_indices);

        //estimate normal
        tspace.clear();
        est_tangent_space_lapack(pt, neighbor_pts, h, tdim, tspace);
		

        projected_v.clear();
        projected_vv[0] = 0;
        projected_vv[1] = 0;
        projected_v.push_back(projected_vv); //// first element of projected_v is [0,0]

        tr.clear();
		vector<Point_2d> nb_2;
		
        for (unsigned int j = 0; j < neighbor_pts.size(); j++)
        {
            dVector vec = neighbor_pts[j] - pt;

            double x = CGAL::to_double(vec * tspace[0]);
            double y = CGAL::to_double(vec * tspace[1]);

            projected_vv[0] = x;
            projected_vv[1] = y;

            Vertex_handle_2d vh = tr.insert(Point_2d(x, y));
            assert(vh != NULL);

            if (vh->get_origin_id() >= 0)
            {
                //cout<<"warning: containing duplicated points"<<endl;
                printf("Containing duplicated points\n");
            }

            vh->set_origin_id(neighbor_indices[j]);

            projected_v.push_back(projected_vv);
        }


        // calculate corresponding Voronoi diagram cell area
        vc_area = voronoi_cell_area(projected_v);

        int v2id;
        dPoint v2;

        /**
         * Diagonal elements is not calculated. Only Gaussian kernel is calculated.
         */
        for (vector<dPoint>::iterator iter = neighbor_pts.begin(); iter != neighbor_pts.end(); iter++)
        {
            map<const dPoint, int, LessdPoint>::iterator
            iter_pt2index = pt2index.find(*iter);

            if (iter_pt2index != pt2index.end())
            {
                v2id = iter_pt2index->second;
                v2 = iter_pt2index->first;

                if (iter_pt2index->second != (int) i)
                {
                    double sqdist = CGAL::to_double((v2 - pt) * (v2 - pt));
                    double weight = exp(-sqdist / hh) / nmfactor;

                    II.push_back(i + 1);
                    JJ.push_back(v2id + 1);
                    SS.push_back(weight);
                }
            }
            else
            {
                printf("generate_sym_pcd_qb: v2=pt in calculation.\n");
            }
        }

        /*
        for (Triangulation_2::Vertex_iterator viter = tr.vertices_begin(); viter != tr.vertices_end(); viter++) {
            unsigned int vid = viter->get_origin_id();
            if (vid == i) {

                assert((viter->point() - Point_2d(0, 0)) * (viter->point() - Point_2d(0, 0)) < FT(1e-5));
                continue;
            }

            double sqdist =
                    CGAL::to_double((viter->point() - Point_2d(0, 0)) * (viter->point() - Point_2d(0, 0)));
            double weight = exp(-sqdist / hh) / nmfactor;



            II.push_back(i + 1);
            JJ.push_back(vid + 1);
            SS.push_back(weight);
        }
         */

        BB.push_back(vc_area);
    }

    printf("\n");
}


////=========================================================================================
////_m_II is vector of row indexes i, _m_JJ is vector of column indexes j
void cal_sym_pcb_qb(PCloud _pc, std::vector<unsigned int>& _m_II, std::vector<unsigned int>& _m_JJ, std::vector<double>& _m_SS,	std::vector<double>& _m_BB, vector<vector<int>>& _vector_neighbor_id, vector<vector<dPoint>>& _vector_neighbor_d)
{
    if (_pc.get_cloud_size() > 10)
    {
        double avgs = _pc.average_size(10);
        printf("Average distance avgs: %f\n\n", avgs);

        //cerr << "cal_sym_pcd_qb to reserve for IJSB .\n";
        _m_II.reserve(_pc.get_cloud_size() * 200);  //minimum capacity for vector m_II
        //cerr << "reservation for I done " << _pc.get_cloud_size() * 180 << endl;
        _m_JJ.reserve(_pc.get_cloud_size() * 200);
       // cerr << "reservation for J done " << _pc.get_cloud_size() * 180 << endl;
        _m_SS.reserve(_pc.get_cloud_size() * 200);
       // cerr << "reservation for S done " << _pc.get_cloud_size() * 180 << endl;
        _m_BB.reserve(_pc.get_cloud_size());
       // cerr << "reservation for B done " << _pc.get_cloud_size() << endl;
       // cerr << "cal_sym_pcd_qb reservation done.\n\n";

        generate_sym_pcd_qb(_pc, avgs * 2, 3, _m_II, _m_JJ, _m_SS, _m_BB, _vector_neighbor_id, _vector_neighbor_d);

        //////////////////////////////////////////////////////////
        /* put average area for all boundary vertices */
        double val, sum = 0;
        int i = 0;

        BOOST_FOREACH(val, _m_BB)
        {
            if (val > 0)
            {
                sum += val;
                ++i;
            }
            else
            {
                printf("cal_sym_pcd_qb: boundary detected.\n");
            }
        }
        sum /= i;

        for (i = 0; i < _m_BB.size(); ++i)
        {
            if (_m_BB[i] <= 0)
            {
                _m_BB[i] = sum;
            }
        }

        /* add area factor for all non-diag elements
        * currently assume that
        * PCDLap code generate symmetric connectivity
        */
        std::vector<double> diag;
        diag.resize(_pc.get_cloud_size(), 0);
        for (i = 0; i < _m_II.size(); ++i)
        {
            if (_m_II[i] == _m_JJ[i])
            {
                cerr << "cal_sym_pcd_qb: errornous diagonal element \n";
                continue;
            }

            if (_m_JJ[i] > _m_II[i])
            {
                _m_SS[i] *= _m_BB[_m_JJ[i] - 1];
                _m_SS[i] *= _m_BB[_m_II[i] - 1];
            }
            else
            {
                _m_SS[i] *= _m_BB[_m_II[i] - 1];
                _m_SS[i] *= _m_BB[_m_JJ[i] - 1];
            }
            diag[_m_II[i] - 1] += _m_SS[i];
        }
        /* add the diagonal elements */
        for (i = 0; i < diag.size(); ++i)
        {
            _m_II.push_back(i + 1);
            _m_JJ.push_back(i + 1);
            _m_SS.push_back(0 - diag[i]);
        }

        //delete pc;
        //delete[] ar;
    }
    else
    {
        std::cout << "Points' number < 10 !" << std::endl;
    }
}



bool save_IJS(const char* filename, std::vector<unsigned int>& II, std::vector<unsigned int>& JJ, std::vector<double>& SS)
{
    if (filename == NULL) { return false; }
    if ((II.size() < SS.size()) || (JJ.size() < SS.size()))
    {
        cerr << "save_IJS: size of parameters do not match: " << II.size() << JJ.size() << SS.size() << endl;
        return false;
    }

    cerr << "save_IJS: parameters valid" << endl;

    std::ofstream ofs;
    char buf[300];
    double val;
    std::string s;

    ofs.open(filename);

    int i = 0;

    cerr << "save_IJS: begin iteration" << endl;
    BOOST_FOREACH(val, SS)
    {
        sprintf(buf, "%d %d %4.20f\n", II[i], JJ[i], val);
        s.clear();
        s.append(buf);

        ofs << s;
        ofs.flush();
        ++i;
    }

    cerr << "save_IJS: end iteration" << endl;

    ofs.close();

    return true;
}



bool writeDoubleArray(std::vector<double>& param, const char* filename)
{
    std::ofstream ofs;
    char buf[300];
    double val;
    std::string s;

    ofs.open(filename);

    BOOST_FOREACH(val, param)
    {
        sprintf(buf, "%4.20f ", val);
        s.clear();
        s.append(buf);

        ofs << s;
    }

    ofs.close();

    return true;
}
